package edu.mapi.ir.planning;

import java.awt.geom.Point2D;
import java.io.File;
import java.util.LinkedHashMap;
import java.util.PriorityQueue;
import java.util.Vector;

import edu.mapi.ir.viewer.Maze;
/**
 * @author ZP
 */
public class AStarSearch {

	protected LinkedHashMap<String, AStarCell> closedList = new LinkedHashMap<String, AStarCell>();
	protected PriorityQueue<AStarCell> openList = new PriorityQueue<AStarCell>();
	protected LinkedHashMap<String, AStarCell> openListMap = new LinkedHashMap<String, AStarCell>();	
	protected int xorigin;
	protected int yorigin;
	protected int xtarget;
	protected int ytarget;
	SimpleMap map;
	
	public AStarSearch(Maze maze) {
		map = new SimpleMap(maze);
	}
	
	public Vector<Point2D> getBestPath(Point2D origin, Point2D target, double initialRotation) {
		xorigin = (int) origin.getX();
		yorigin = (int) origin.getY();
		xtarget = (int) target.getX();
		ytarget = (int) target.getY();
		
		AStarCell org = new AStarCell(xorigin, yorigin,10*(Math.abs(xtarget-xorigin)+Math.abs(ytarget-yorigin)), 0);
		org.setRotation(initialRotation);
		openList.add(org);
		openListMap.put(org.toString(), org);
		
		AStarCell cell = doSearch();
		
		if (cell == null)
			return null;
		else {
			Vector<Point2D> path = new Vector<Point2D>();
			Point2D pt = new Point2D.Double(cell.x+0.5, cell.y+0.5);
			path.add(pt);
			
			while (cell.getParent() != null) {
				cell = cell.getParent();
				pt = new Point2D.Double(cell.x+0.5, cell.y+0.5);
				path.add(0, pt);
			}
			path.removeElementAt(0);
			//path.get(0).setLocation(origin);
			path.lastElement().setLocation(target);
			return path;
		}
	}
	
	
	
	
	int steps = 1;
	private AStarCell doSearch() {
				
		while (openList.peek() != null) {
			AStarCell pivot = openList.poll();
	
			openList.remove(pivot);
			openListMap.remove(pivot.toString());
			
			closedList.put(pivot.toString(), pivot);
			
			if (pivot.x == xtarget && pivot.y == ytarget)
				return pivot;
			
			System.out.println("Analyzing "+pivot.debugString());
			
			for (int k = -1; k <= 1; k++) {								
				for (int l = -1; l <= 1; l++) {

					int i = pivot.x + k;
					int j = pivot.y + l;

					if (k == 0 && l == 0)
						continue;
					
					if (i < 0 || i >= map.getMaze().getWidth())
						continue;
					
					if (j < 0 || j >= map.getMaze().getHeight())
						continue;
					
					
					double distance2pivot = (i != pivot.x && j != pivot.y)? 14 : 10;
					AStarCell cell = new AStarCell(i, j,
							10 * (Math.abs(i-xtarget) + Math.abs(j-ytarget))
							/*Point2D.distance(i, j, xtarget, ytarget)*/, pivot.distance2origin+distance2pivot);
					if (closedList.containsKey(cell.toString())) {
						continue;
					}
					
					if (map.isObstacle(i,j)){
						//System.out.println("Obstacle found in ["+i+","+j+"]");						
						closedList.put(cell.toString(),cell);
						continue;
					}
					
					if(k != 0 && l != 0 && (map.isObstacle(pivot.x,j) || map.isObstacle(i,pivot.y))) {
						System.out.println("Obstacle to ["+i+","+j+"] found");
						continue;
					}
					
					//	closedList.put(cell.toString(),cell);
					//	continue;
					//}
					
					if (openListMap.containsKey(cell.toString())){
						AStarCell previousCell = openListMap.get(cell.toString());
						if (previousCell.compareTo(cell) > 0) {							
							cell.setParent(pivot);
							openListMap.put(cell.toString(), cell);
							openList.add(cell);
							//System.out.println("The value of cell ["+cell.x+", "+cell.y+"] was redefined.");							
						}
						else {
							//System.out.println("The value for cell ["+cell.x+", "+cell.y+"] was previously better");
						}
						 //System.out.println("from \t(before="+previousCell.getValue()+"  now="+cell.getValue());
						continue;
					}
					cell.setParent(pivot);

					openList.add(cell);
					openListMap.put(cell.toString(), cell);
					//System.out.println("\t Add "+cell.debugString());					
				}
			}
		}
		//System.out.println("Unable to go to ["+xtarget+", "+ytarget+"]");
		return null;
	}
	
	public static void main(String[] args) throws Exception {
		final Maze m = Maze.readXML(new File("c:/maze1.xml"), new File("c:/grid.xml"));
		Vector<Point2D> res = new AStarSearch(m).getBestPath(new Point2D.Double(0,0), new Point2D.Double(15,12), 0);
		if (res != null)
			for (int i = 0; i < res.size(); i++)
				System.out.println(res.get(i));
	}	
}
